#ifndef SORT_DATASHARER_H
#define SORT_DATASHARER_H

#include <pcl/visualization/cloud_viewer.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/io/openni_grabber.h>

#include <mrpt/base.h>

namespace SORT
{
	namespace DataIO
	{
		class SimpleViewer
		{
		   public:
			 SimpleViewer () : viewer ("PCL OpenNI Viewer") {}

			 void cloud_cb_ (const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &cloud)
			 {
			   if (!viewer.wasStopped())
				 viewer.showCloud (cloud);

			   static size_t one = 0;
			 }

			 void run ()
			 {
			   pcl::Grabber* interface = new pcl::OpenNIGrabber();

			   boost::function<void (const pcl::PointCloud<pcl::PointXYZ>::ConstPtr&)> f =
				 boost::bind (&SimpleViewer::cloud_cb_, this, _1);

			   interface->registerCallback (f);

			   interface->start ();

			   while (!viewer.wasStopped())
			   {
				 boost::this_thread::sleep (boost::posix_time::seconds (1));
			   }

			   interface->stop ();
			 }

			 pcl::visualization::CloudViewer viewer;
		 };

		class SensorDataSharer
		{
		private:
			pcl::Grabber*	m_interface;
			bool			m_isRunning;
			mrpt::synch::CCriticalSection semaphore; //!< Semaphore for ensuring data consistency
			size_t			m_ID, m_lastIDChecked;
			pcl::PointCloud<pcl::PointXYZ>::Ptr m_cloud;

		public:
			 
			SensorDataSharer(): m_isRunning(false), m_ID(0), m_lastIDChecked(0), m_cloud(new pcl::PointCloud<pcl::PointXYZ>)
			{
				m_interface = new pcl::OpenNIGrabber();

				boost::function<void (const pcl::PointCloud<pcl::PointXYZ>::ConstPtr&)> f =
				boost::bind (&SensorDataSharer::cloudRetriever, this, _1);

				m_interface->registerCallback (f);
			}

			~SensorDataSharer()
			{
				if ( m_isRunning )
					stop();
			}
		     
			void cloudRetriever(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &cloud)
			{
				mrpt::synch::CCriticalSectionLocker cs( &semaphore );
				pcl::copyPointCloud(*cloud,*m_cloud);
				//pcl::PointCloud<pcl::PointXYZ>::Ptr m_cloud = new pcl::PointCloud<pcl::PointXYZ>(cloud);
				m_ID = m_ID++ % 1000;
			}

			void run ()
			{		
				m_interface->start();
				m_isRunning = true;
			}

			void stop()
			{
				m_interface->stop();
				m_isRunning = false;
			}

			bool getLastPointCloud( pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud )
			{
				mrpt::synch::CCriticalSectionLocker cs( &semaphore );
				if ( m_ID == m_lastIDChecked )
					return false;
				
				pcl::copyPointCloud(*m_cloud,*cloud);
				
				m_lastIDChecked = m_ID;

				return true;
			}

		 };
	}
}

#endif